/*
 * Copyright (c) 2011-2014, fortiss GmbH.
 * Licensed under the Apache License, Version 2.0.
 *
 * Use, modification and distribution are subject to the terms specified
 * in the accompanying license file LICENSE.txt located at the root directory
 * of this software distribution. A copy is available at
 * http://chromosome.fortiss.org/.
 *
 * This file is part of CHROMOSOME.
 *
 * $Id: getDifferenceFunction.cpp 7805 2014-03-13 09:54:35Z geisinger $
 */

/**
 * \file
 *         Source file for function getDifference in component proxy.
 *
 * \author
 *         This file has been generated by the CHROMOSOME Modeling Tool (XMT)
 *         (fortiss GmbH).
 */

/******************************************************************************/
/***   Includes                                                             ***/
/******************************************************************************/
#include "rOSGateway/adv/proxy/include/getDifferenceFunction.h"

#include "rOSGateway/adv/proxy/include/getDifferenceFunctionWrapper.h"
#include "rOSGateway/adv/proxy/include/proxyComponent.h"
#include "rOSGateway/adv/proxy/include/proxyComponentWrapper.h"
#include "rOSGateway/adv/proxy/include/proxyManifest.h"

#include "xme/core/logUtils.h"

#include "xme/hal/include/mem.h"

// PROTECTED REGION ID(ROSGATEWAY_ADV_PROXY_GETDIFFERENCEFUNCTION_C_INCLUDES) ENABLED START
#pragma GCC diagnostic ignored "-Wshadow" //TODO find better solution
#include "ros/ros.h"

#include "xme/defines.h"
#include "xme/hal/include/sched.h" //TODO
#include "xme/hal/include/time.h" //TODO

#include "ros_system/SubstractTwoInts.h"
// PROTECTED REGION END

/******************************************************************************/
/***   Definitions                                                          ***/
/******************************************************************************/

// PROTECTED REGION ID(ROSGATEWAY_ADV_PROXY_GETDIFFERENCEFUNCTION_C_DEFINITIONS) ENABLED START
// PROTECTED REGION END

/******************************************************************************/
/***   Variables                                                            ***/
/******************************************************************************/

// PROTECTED REGION ID(ROSGATEWAY_ADV_PROXY_GETDIFFERENCEFUNCTION_C_VARIABLES) ENABLED START
static ros::NodeHandle* nodeHandle;

static ros::ServiceClient difference_client;
static ros_system::SubstractTwoInts srv;

static xme_core_component_requestDataHandle_t getDifferenceRequestHandle;
// PROTECTED REGION END

/******************************************************************************/
/***   Prototypes                                                           ***/
/******************************************************************************/

// PROTECTED REGION ID(ROSGATEWAY_ADV_PROXY_GETDIFFERENCEFUNCTION_C_PROTOTYPES) ENABLED START
static void getDifferenceTaskCallback(void* userData);
// PROTECTED REGION END

/******************************************************************************/
/***   Implementation                                                       ***/
/******************************************************************************/
xme_status_t
rOSGateway_adv_proxy_getDifferenceFunction_init
(
    rOSGateway_adv_proxy_proxyComponent_config_t* const componentConfig
)
{
    // PROTECTED REGION ID(ROSGATEWAY_ADV_PROXY_GETDIFFERENCEFUNCTION_INITIALIZE_C) ENABLED START
    XME_UNUSED_PARAMETER(componentConfig);

    nodeHandle = new ros::NodeHandle();

    difference_client = nodeHandle->serviceClient<ros_system::SubstractTwoInts>("substract_two_ints");

    return XME_STATUS_SUCCESS;
    // PROTECTED REGION END
}

void
rOSGateway_adv_proxy_getDifferenceFunction_step
(
    rOSGateway_adv_proxy_proxyComponent_config_t* const componentConfig
)
{
    xme_status_t status[1];
    
    ROSGateway_topic_differenceRequest_t portGetDifferenceData; // Required port.
    xme_core_component_requestDataHandle_t portGetDifferenceRequestDataHandle = (xme_core_component_requestDataHandle_t)XME_CORE_COMPONENT_INVALID_REQUEST_DATA_HANDLE;
    
    (void)xme_hal_mem_set(&portGetDifferenceData, 0u, sizeof(ROSGateway_topic_differenceRequest_t));
    
    status[0] = rOSGateway_adv_proxy_proxyComponentWrapper_readRequestHandlerPortGetDifference(&portGetDifferenceData, &portGetDifferenceRequestDataHandle);
    
    {
        // PROTECTED REGION ID(ROSGATEWAY_ADV_PROXY_GETDIFFERENCEFUNCTION_STEP_C) ENABLED START
        XME_UNUSED_PARAMETER(componentConfig);
        XME_UNUSED_PARAMETER(status);

        xme_fallback_printf("Proxy: getDifference\n");

		srv.request.a = portGetDifferenceData.a;
		srv.request.b = portGetDifferenceData.b;

		getDifferenceRequestHandle = portGetDifferenceRequestDataHandle;

		if (XME_HAL_SCHED_INVALID_TASK_HANDLE == xme_hal_sched_addTask(xme_hal_time_timeIntervalFromMilliseconds(0), xme_hal_time_timeIntervalFromMilliseconds(0), 0, getDifferenceTaskCallback, NULL))
		{
			XME_EXIT(EXIT_FAILURE, "Error creating getDifferenceThread\n");
		}
        // PROTECTED REGION END
    }
    
    {
        // PROTECTED REGION ID(ROSGATEWAY_ADV_PROXY_GETDIFFERENCEFUNCTION_STEP_2_C) ENABLED START
        // PROTECTED REGION END
    }
}

void
rOSGateway_adv_proxy_getDifferenceFunction_fini
(
    rOSGateway_adv_proxy_proxyComponent_config_t* const componentConfig
)
{
    // PROTECTED REGION ID(ROSGATEWAY_ADV_PROXY_GETDIFFERENCEFUNCTION_TERMINATE_C) ENABLED START
    XME_UNUSED_PARAMETER(componentConfig);
    
	delete nodeHandle;
	nodeHandle = NULL;
    // PROTECTED REGION END
}

// PROTECTED REGION ID(ROSGATEWAY_ADV_PROXY_GETDIFFERENCEFUNCTION_IMPLEMENTATION_C) ENABLED START
static void getDifferenceTaskCallback(void* userData)
{
	xme_status_t status;
	ROSGateway_topic_differenceResponse_t portSendDifferenceData;
    ROSGateway_topic_differenceResponse_t* portSendDifferenceDataPtr = &portSendDifferenceData;

	XME_UNUSED_PARAMETER(userData);

	if (difference_client.call(srv))
	{
        xme_fallback_printf("Proxy: receivedDifference\n");
        portSendDifferenceData.difference = srv.response.difference;
	}
	else
	{
		XME_EXIT(EXIT_FAILURE, "Failed to call substract_two_ints!\n");
	}

	status = rOSGateway_adv_proxy_proxyComponentWrapper_writeResponseSenderPortSendDifference(portSendDifferenceDataPtr, &getDifferenceRequestHandle);
	if (XME_STATUS_SUCCESS != status)
	{
		XME_LOG(XME_LOG_ERROR, "getDifferenceThread: Error sending response!\n");
	}

	rOSGateway_adv_proxy_proxyComponentWrapper_completeWriteOperations();

}
// PROTECTED REGION END
